#include <Servo.h>
#include <Arduino.h>
#include <Wire.h>
#include <U8g2lib.h>

U8G2_SSD1306_128X64_NONAME_F_SW_I2C U8g2(U8G2_R0, SCL, SDA, U8X8_PIN_NONE);

int servoPin = 9;                // servo pin
int val;                         // angle set
int ledPinMatriex[] = { 5, 6 };  // led pin

Servo servo;  // get a servo object

const int TrigPin = 11;
const int EchoPin = 12;
int distance = 0;
int timeCount = 0;
int middleAngle = 90;
int angleCount = 1;

// init pins
void setup() {
  // servo init
  servo.attach(servoPin);  // reset
  // serial init
  Serial.begin(9600);
  // led init
  for (int i = 0; i < 2; i++) {
    pinMode(ledPinMatriex[i], OUTPUT);
    analogWrite(ledPinMatriex[i], 0);
  }
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  // lcd
  U8g2.begin();
}

// lcd write information
// x:the line
// y:the col
void lcdWrite(int x, int y, String str) {
  U8g2.drawStr(x, y, str.c_str());
  U8g2.sendBuffer();
}

// get Serial String
String getSerialString() {
  String comdStr = "";
  while (Serial.available()) {
    char tmp = Serial.read();
    comdStr += tmp;
  }
  return comdStr;
}

// control
void control(int angle) {
  // limit the angle range
  if (angle >= 180)
    angle = 180;
  if (angle <= 0)
    angle = 0;
  servo.write(angle);
  String str = "Servo's angle is ";
  String tmp = String(angle);
  str += tmp;
  lcdWrite(1, 30, str);
  if (angle > 90) {
    analogWrite(ledPinMatriex[0], 0);
    analogWrite(ledPinMatriex[1], 255);
    return;
  }
  servo.write(angle);
  str = "Servo's angle is ";
  tmp = String(angle);
  str += tmp;
  lcdWrite(1, 30, str);
  if (angle < 90) {
    analogWrite(ledPinMatriex[1], 0);
    analogWrite(ledPinMatriex[0], 255);
    return;
  }
}

// loop function
void loop() {
  control(90);
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(100);
  digitalWrite(TrigPin, LOW);
  distance = pulseIn(EchoPin, HIGH) / (1000 / 340 * 2);
  U8g2.setFont(u8g2_font_ncenB08_tr);
  String str = "The distance is ";
  str += distance;
  str += " cm";
  lcdWrite(0, 10, str);
  String comdStr = getSerialString();
  char comd = comdStr[0];
  switch (comd) {
    case 't':
      {
        int angle = comdStr.substring(0, comdStr.length() - 1).toInt();
        control(angle);
      }
      break;
    case 's':
      {
        control(90);
        analogWrite(ledPinMatriex[1], 0);
        analogWrite(ledPinMatriex[0], 0);
      }
      break;
  }
  timeCount += 1;
  if (timeCount / 10) {
    control(middleAngle - angleCount);
    control(middleAngle + angleCount);
    angleCount += 30;
    if (angleCount == 90)
      angleCount = 0;
    timeCount = 0;
  }
}
